/**
 * @file           : powerCtrl.h
 * @author         : Star_Plucking
 * @brief          : 功控库
 * @date           : 25-2-28 下午5:32
 * @lastEditor     :
 * @lastEditTime   : 25-2-28 下午5:32
 */

#pragma once

#include "algorithm/filter.h"
#include "algorithm/math.h"
#include "algorithm/ringbuff.h"
#include "algorithm/rls.hpp"
#include <cmath>

namespace os::power {
using namespace sjtu;

class PowerCtrl {
private:
    math::rls<2> rls_;
    math::SlidingWindow<float, 8> speed_window_[4];
    float omega_[4]{};
    filter::LowPassFilter current_filer_[4] = {filter::LowPassFilter(50.0F), filter::LowPassFilter(50.0F),
                                               filter::LowPassFilter(50.0F), filter::LowPassFilter(50.0F)};
    filter::LowPassFilter output_filter_[4] = {filter::LowPassFilter(50.0F), filter::LowPassFilter(50.0F),
                                               filter::LowPassFilter(50.0F), filter::LowPassFilter(50.0F)};
    filter::LowPassFilter power_filter_ = filter::LowPassFilter(50.0F);
    float cal_omega_[4]{};
    float power_{};
    float cal_omega_update_[4]{};
    float cal_current_update_[4]{};
    float const K3_CONSTANT_ = 0.3F;
    float const MAX_CURRENT_ = 20.0F; // 最大电流值
    float const TORQUE_CURRENT_RATIO_ = 0.5F; // 扭矩电流比

public:
    PowerCtrl() : rls_(1e-5F, 0.9999F) {
    }

    ~PowerCtrl() = default;

    /**
     * @brief 重置参数
     *
     */
    void reset() { rls_.reset(); }

    /**
     * @brief 根据电流和转速预测功率
     * @param current 当前电流值
     * @param speed 当前速度值
     * @return float 预测功率值
     */
    float32_t getPredictPower(float current, float speed) const {
        float _[2] = {speed, current * current};
        Matrixf<2, 1> sample(_);
        return (rls_.getParamsVector().trans() * sample)[0][0];
    }

    /**
     * @brief 从功率和当前车辆转速推导电流
     * @param power 目标功率
     * @param speed 当前转速
     * @param old_current 上次电流
     * @return 当前转速下输出目标功率所需要的电流
     */
    float getCurrentFromPower(float const power, float const speed, float const old_current) const {
        auto params = rls_.getParamsVector();
        // 将力矩τ表示为电流I的函数: τ = I * TORQUE_CURRENT_RATIO
        // 代入方程: k2*τ^2 + ω*τ + k1*|ω| + k3/n - P = 0
        // 得到: k2*(I*RATIO)^2 + ω*(I*RATIO) + k1*|ω| + k3/n - P = 0
        // 整理得: (k2*RATIO^2)*I^2 + (ω*RATIO)*I + (k1*|ω| + k3/n - P) = 0

        // 二次方程系数
        float const a = params[1][0] * TORQUE_CURRENT_RATIO_ * TORQUE_CURRENT_RATIO_; // k2*RATIO^2
        float const b = speed * TORQUE_CURRENT_RATIO_; // ω*RATIO
        float const c = (params[0][0] * fabsf(speed)) + (K3_CONSTANT_ / 4.0F) - power; // k1*|ω| + k3/4 - P

        // 检查特殊情况：A接近0
        if (fabsf(a) < 1e-6F) {
            // 退化为线性方程: B*I + C = 0
            if (fabsf(b) > 1e-6F) {
                return -c / b;
            }
            // 无法求解的情况
            return 0.0F;
        }

        //  计算Δ
        float const delta = (b * b) - (4.0F * a * c);

        float new_current{};

        if (delta < 0.0F) {
            new_current = old_current; // 无解,置零
        } else {
            float sqrt_delta = sqrtf(delta);

            if (old_current >= 0) {
                new_current = (-b + sqrt_delta) / (2.0F * a); // 正速度对应正电流
            } else {
                new_current = (-b - sqrt_delta) / (2.0F * a); // 负速度对应负电流
            }

            // 检查选择的解是否与当前电流方向一致
            // 如果方向不一致且有两个有效解，则选择另一个解
            if ((new_current * old_current < 0) and (delta > 0.0F)) {
                if (old_current >= 0) {
                    new_current = (-b + sqrt_delta) / (2.0F * a);
                } else {
                    new_current = (-b - sqrt_delta) / (2.0F * a);
                }

                // 如果还是方向不一致，则可能系统状态发生了变化
                // 选择更接近零的解或减小当前电流
                if (new_current * old_current < 0) {
                    new_current = old_current * 0.5F;
                }
            }
        }

        LimitMaxMin(new_current, MAX_CURRENT_, -MAX_CURRENT_); // 限制电流范围

        return new_current;
    }

    /**
     * @brief 功率控制主函数算法
     * @param powerTarget 目标功率（单位：W）
     * @param currentToSend 待发送电流数据（单位：A）
     * @param speedNow 当前速度数据（输出轴，单位 rad/s）
     * @param measurePower 当前功率(读取自功率计)（单位：W）
     * @param currentNow jhtSB
     */
    void calculator(float const powerTarget, float currentToSend[4], float const speedNow[4], float measurePower,
                    float currentNow[4]) {
        auto params = rls_.getParamsVector();

        float speed[4]{}; // 滤波后的速度
        float current[4]{}; // 当前电流
        float torque[4]{}; // 力矩
        float speed_abs[4]{}; // 速度绝对值
        float torque_squared[4]{}; // 力矩平方
        float mechanical_power[4] = {}; // 机械功率
        float power_predictions[4]{}; // 预测功率
        float sum_power{}; // 总预测功率

        // 计算每个轮电机的预测功率
        for (int i = 0; i < 4; i++) {
            // using math::SlidingWindow;  // 导入类
            // using math::operator<<;
            // 速度滑窗滤波
            cal_omega_[i] -= speed_window_[i][0];
            speed_window_[i] << speedNow[i];
            cal_omega_[i] += speed_window_[i][7];

            speed[i] = cal_omega_[i] / 8.0F;
            current[i] = currentToSend[i];

            torque[i] = current[i] * TORQUE_CURRENT_RATIO_;
            speed_abs[i] = fabsf(speed[i]);
            torque_squared[i] = torque[i] * torque[i];
            mechanical_power[i] = torque[i] * speed[i];

            power_predictions[i] =
                mechanical_power[i] + params[0][0] * speed_abs[i] + params[1][0] * torque_squared[i] + K3_CONSTANT_ /
                4.0F;

            sum_power += power_predictions[i];
        }
        if (sum_power > powerTarget) {
            for (int i = 0; i < 4; i++) {
                float old_current = current[i];
                float now_speed = speed[i];

                // 只缩放正功率
                if (now_speed * old_current > 0) {
                    float power_ratio = power_predictions[i] / sum_power;
                    float target_power = (powerTarget * power_ratio) - (K3_CONSTANT_ / 4.0F);

                    float new_current = getCurrentFromPower(target_power, now_speed, old_current);
                    cal_current_update_[i] = new_current;
                }
            } // end of for
        } // 功率小于目标功率
        else {
            for (int i = 0; i < 4; i++) {
                cal_current_update_[i] = current[i];
            }
        } // end of else if

        for (short i = 0; i < 4; i++) {
            currentToSend[i] = cal_current_update_[i];
        }

        // 更新RLS模型
        // 构建特征向量用于RLS更新
        float actual_speed_abs = 0.0F;
        float actual_torque_squared = 0.0F;
        float actual_mechanical_power = 0.0F;

        for (int i = 0; i < 4; i++) {
            float actual_current = current_filer_[i].calculate(currentNow[i]);
            float actual_torque = actual_current * TORQUE_CURRENT_RATIO_;

            // 累加特征值
            actual_speed_abs += fabsf(speed[i]);
            actual_torque_squared += actual_torque * actual_torque;
            actual_mechanical_power += actual_torque * speed[i];
        }

        Matrixf<2, 1> sample;
        sample[0][0] = actual_speed_abs;
        sample[1][0] = actual_torque_squared;

        measurePower = power_filter_.calculate(measurePower);

        // 添加死区判断和负功率保护
        // 1. 确保测量功率幅值足够大，避免小信号干扰
        // 2. 避免使用明显不合理负功率值更新模型

        float estimated_power = actual_mechanical_power + (params[0][0] * actual_speed_abs) +
                                (params[1][0] * actual_torque_squared) + (K3_CONSTANT_ / 4.0F);
        if (fabsf(measurePower) > 5.0F and (estimated_power >= 0 || measurePower >= 0)) {
            auto cal_params = rls_.update(sample, measurePower - actual_mechanical_power - K3_CONSTANT_);
            // 更新RLS参数

            // 确保参数不会出现负值，可能导致模型不稳定
            if (cal_params[0][0] < 1e-5F) {
                // 重置为最小正数，避免参数发散
                Matrixf<2, 1> new_sample;
                new_sample[0][0] = fmaxf(cal_params[0][0], 1e-5F);
                new_sample[1][0] = cal_params[1][0];
                rls_.setParamVector(new_sample);
            }

            if (cal_params[1][0] < 1e-5F) {
                // 重置为最小正数，避免参数发散
                Matrixf<2, 1> new_sample;
                new_sample[0][0] = cal_params[0][0];
                new_sample[1][0] = fmaxf(cal_params[1][0], 1e-5F);
                rls_.setParamVector(new_sample);
            } // end if of cal_params[1][0]
        } // end if of measurePower
    } // end of calculator

    math::rls<2>& Get_rls() {
        return rls_;
    }
};
} // namespace os::power